/*
 * Copyright (C) 2010 Johan Waldeback
 *  
 * This file is part of the OpenFMS project (http://www.openfms-project.org)
 * 
 * OpenFMS is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "Testing.h"
#include "SweepDialog.h"
#include <tchar.h>
using namespace OpenFms;

extern bool parseFloatString( float &result,  const TCHAR *str );
extern float interpolate( int n, int steps, float a, float b );
extern float calcYawPitchX( float yaw, float pitch, float x, float y, float z );
extern float calcYawPitchY( float yaw, float pitch, float x, float y, float z );
extern float calcYawPitchZ( float yaw, float pitch, float x, float y, float z );


/**
 * Helper function used when testing readFloat(float &, const TCHAR*)
 *
 * This function passes on the string to the readFloat function and 
 * returns the parsed value. Note: this function does not indicate if the parsing 
 * failed, and will retrun something undefined if that is the case
 *
 * @param p string parameter to be past on to the readFloat function
 * @return Returns the parsed result returned from readFloat, if readFloat failes, the return 
 * value is undefined (and with no possibillity to detemine a fail occured)
 */
static float parseFloatStringTest(const TCHAR *p) {
	float f;
	parseFloatString( f, p );
	return f;
}

/**
 * Helper function used when testing readFloat(float &, const TCHAR*)
 *
 * This function passes on the string to the readFloat function and 
 * returns if readFloat says it was able to pare the string or not.
 * Note: The actual pared value is ignored.
 *
 * @param p string parameter to be past on to the readFloat function
 * @return Returns true if readFloat returns true. Returns false if readFloat returns false.
 */
static bool parseFloatStringTestSuccess(const TCHAR *p) {
	float f;
	return parseFloatString( f, p );
}

/**
 * Contant PI (approx 3.14)
 */
static const float pi = 4* atan(1.f);

/*
 * Sets SweepDialog instance variables to default values
 *
 * Used for tetsing - Sets SweepDialog instance variables to default/known/defined variables.
 * Note: requires SweepDialog source to be compiled with MAKE_TEST defined.
 */
static void resetInstanceVariables( SweepDialog &dlg )
{
	dlg.setValues( SweepDialog::ID_WINDX, 0.f, 0.f, 1.f );
	dlg.setValues( SweepDialog::ID_WINDY, 0.f, 0.f, 1.f );
	dlg.setValues( SweepDialog::ID_WINDZ, 0.f, 0.f, 1.f );
	dlg.setValues( SweepDialog::ID_WINDYAW, 0.f, -pi, pi );
	dlg.setValues( SweepDialog::ID_WINDPITCH, 0.f, -pi, pi );
	dlg.setValues( SweepDialog::ID_CHANNEL1, 0.f, -1.f, 1.f );
	dlg.setValues( SweepDialog::ID_CHANNEL2, 0.f, -1.f, 1.f );
	dlg.setValues( SweepDialog::ID_CHANNEL3, 0.f, -1.f, 1.f );
	dlg.setValues( SweepDialog::ID_CHANNEL4, 0.f, -1.f, 1.f );
	dlg.setValues( SweepDialog::ID_CHANNEL5, 0.f, -1.f, 1.f );
	dlg.setValues( SweepDialog::ID_CHANNEL6, 0.f, -1.f, 1.f );
	dlg.setValues( SweepDialog::ID_CHANNEL7, 0.f, -1.f, 1.f );
	dlg.setValues( SweepDialog::ID_CHANNEL8, 0.f, -1.f, 1.f );
	dlg.setNumberOfSteps(101);
	dlg.setSelectedInput( SweepDialog::ID_CHANNEL1 );
	dlg.setSelectedOutput( SweepDialog::ID_FORCE_NEGZ );
}


/**
 * Unit tests for interpolate(...)
 */
TESTSET(testsetInterpolate)
{
	UNITTEST_FLOAT_EQUALS( 0.0f, interpolate( 0, 11, 0.0f, 1.0f) )
	UNITTEST_FLOAT_EQUALS( 0.1f, interpolate( 1, 11, 0.0f, 1.0f) )
	UNITTEST_FLOAT_EQUALS( 0.2f, interpolate( 2, 11, 0.0f, 1.0f) )
	UNITTEST_FLOAT_EQUALS( 0.3f, interpolate( 3, 11, 0.0f, 1.0f) )
	UNITTEST_FLOAT_EQUALS( 0.4f, interpolate( 4, 11, 0.0f, 1.0f) )
	UNITTEST_FLOAT_EQUALS( 0.5f, interpolate( 5, 11, 0.0f, 1.0f) )
	UNITTEST_FLOAT_EQUALS( 0.6f, interpolate( 6, 11, 0.0f, 1.0f) )
	UNITTEST_FLOAT_EQUALS( 0.7f, interpolate( 7, 11, 0.0f, 1.0f) )
	UNITTEST_FLOAT_EQUALS( 0.8f, interpolate( 8, 11, 0.0f, 1.0f) )
	UNITTEST_FLOAT_EQUALS( 0.9f, interpolate( 9, 11, 0.0f, 1.0f) )
	UNITTEST_FLOAT_EQUALS( 1.0f, interpolate( 10, 11, 0.0f, 1.0f) )

	UNITTEST_FLOAT_EQUALS( 10.0f, interpolate( 0, 11, 10.0f, -10.0f) )
	UNITTEST_FLOAT_EQUALS( 8.0f, interpolate( 1, 11, 10.0f, -10.0f) )
	UNITTEST_FLOAT_EQUALS( 6.0f, interpolate( 2, 11, 10.0f, -10.0f) )
	UNITTEST_FLOAT_EQUALS( 4.0f, interpolate( 3, 11, 10.0f, -10.0f) )
	UNITTEST_FLOAT_EQUALS( 2.0f, interpolate( 4, 11, 10.0f, -10.0f) )
	UNITTEST_FLOAT_EQUALS( 0.0f, interpolate( 5, 11, 10.0f, -10.0f) )
	UNITTEST_FLOAT_EQUALS( -2.0f, interpolate( 6, 11, 10.0f, -10.0f) )
	UNITTEST_FLOAT_EQUALS( -4.0f, interpolate( 7, 11, 10.0f, -10.0f) )
	UNITTEST_FLOAT_EQUALS( -6.0f, interpolate( 8, 11, 10.0f, -10.0f) )
	UNITTEST_FLOAT_EQUALS( -8.0f, interpolate( 9, 11, 10.0f, -10.0f) )
	UNITTEST_FLOAT_EQUALS( -10.0f, interpolate( 10, 11, 10.0f, -10.0f) )
}

/**
 * Unit tests for parseFloatString(...)
 */
TESTSET( testsetParseFloatString )
{
	UNITTEST_FLOAT_EQUALS( 1.f, parseFloatStringTest(_T("1.0")))
	UNITTEST_FLOAT_EQUALS( -1.f, parseFloatStringTest(_T("-1.0")))
	UNITTEST_FLOAT_EQUALS( 1.f, parseFloatStringTest(_T("+1.0")))
	UNITTEST_FLOAT_EQUALS(  -123000.0f, parseFloatStringTest(_T("-123000.0")))
	UNITTEST_FLOAT_EQUALS( 123000.0f, parseFloatStringTest(_T("123000.0")))
	UNITTEST_FLOAT_EQUALS( 12.34f, parseFloatStringTest(_T("12.34")))
	UNITTEST_FLOAT_EQUALS( 0.001234f, parseFloatStringTest(_T("0.001234")))

	UNITTEST_FLOAT_EQUALS( 1.f, parseFloatStringTest(_T("1,0")))
	UNITTEST_FLOAT_EQUALS( -1.f, parseFloatStringTest(_T("-1,0")))
	UNITTEST_FLOAT_EQUALS( 1.f, parseFloatStringTest(_T("+1,0")))
	UNITTEST_FLOAT_EQUALS(  -123000.0f, parseFloatStringTest(_T("-123000,0")))
	UNITTEST_FLOAT_EQUALS( 123000.0f, parseFloatStringTest(_T("123000,0")))
	UNITTEST_FLOAT_EQUALS( 12.34f, parseFloatStringTest(_T("12,34")))
	UNITTEST_FLOAT_EQUALS( 0.001234f, parseFloatStringTest(_T("0,001234")))

	UNITTEST_BOOL_EQUALS( true, parseFloatStringTestSuccess(_T("1.0")))
	UNITTEST_BOOL_EQUALS( true, parseFloatStringTestSuccess(_T("-1.0")))
	UNITTEST_BOOL_EQUALS( true, parseFloatStringTestSuccess(_T("+1.0")))
	UNITTEST_BOOL_EQUALS( true, parseFloatStringTestSuccess(_T("-123000.0")))
	UNITTEST_BOOL_EQUALS( true, parseFloatStringTestSuccess(_T("123000.0")))
	UNITTEST_BOOL_EQUALS( true, parseFloatStringTestSuccess(_T("12.34")))
	UNITTEST_BOOL_EQUALS( true, parseFloatStringTestSuccess(_T("0.001234")))

	UNITTEST_BOOL_EQUALS( true, parseFloatStringTestSuccess(_T("1,0")))
	UNITTEST_BOOL_EQUALS( true, parseFloatStringTestSuccess(_T("-1,0")))
	UNITTEST_BOOL_EQUALS( true, parseFloatStringTestSuccess(_T("+1,0")))
	UNITTEST_BOOL_EQUALS( true, parseFloatStringTestSuccess(_T("-123000,0")))
	UNITTEST_BOOL_EQUALS( true, parseFloatStringTestSuccess(_T("123000,0")))
	UNITTEST_BOOL_EQUALS( true, parseFloatStringTestSuccess(_T("12,34")))
	UNITTEST_BOOL_EQUALS( true, parseFloatStringTestSuccess(_T("0,001234")))
}

/**
 * Unit tests for calcYawPitchX/Y/Z(...)
 */
TESTSET( testsetCalcYawPitch )
{
	UNITTEST_FLOAT_EQUALS( 1, calcYawPitchX( 0, 0, 1, 0, 0 ) )
	UNITTEST_FLOAT_EQUALS( 0, calcYawPitchX( 0, 0, 0, 1, 0 ) )
	UNITTEST_FLOAT_EQUALS( 0, calcYawPitchX( 0, 0, 0, 0, 1 ) )
	UNITTEST_FLOAT_EQUALS( sqrt(.5f), calcYawPitchX( pi/4, 0, 1, 0, 0 ) )
	UNITTEST_FLOAT_EQUALS( -sqrt(.5f), calcYawPitchX( pi/4, 0, 0, 1, 0 ) )
	UNITTEST_FLOAT_EQUALS( 0, calcYawPitchX( pi/4, 0, 0, 0, 1 ) )
	UNITTEST_FLOAT_EQUALS( sqrt(.5f), calcYawPitchX( 0, pi/4, 1, 0, 0 ) )
	UNITTEST_FLOAT_EQUALS( 0, calcYawPitchX( 0, pi/4, 0, 1, 0 ) )
	UNITTEST_FLOAT_EQUALS( sqrt(.5f), calcYawPitchX( 0, pi/4, 0, 0, 1 ) )
	UNITTEST_FLOAT_EQUALS( .5f, calcYawPitchX( pi/4, pi/4, 1, 0, 0 ) )
	UNITTEST_FLOAT_EQUALS( -sqrt(.5f), calcYawPitchX( pi/4, pi/4, 0, 1, 0 ) )
	UNITTEST_FLOAT_EQUALS( .5f, calcYawPitchX( pi/4, pi/4, 0, 0, 1 ) )

	UNITTEST_FLOAT_EQUALS( 0, calcYawPitchY( 0, 0, 1, 0, 0 ) )
	UNITTEST_FLOAT_EQUALS( 1, calcYawPitchY( 0, 0, 0, 1, 0 ) )
	UNITTEST_FLOAT_EQUALS( 0, calcYawPitchY( 0, 0, 0, 0, 1 ) )
	UNITTEST_FLOAT_EQUALS( sqrt(.5f), calcYawPitchY( pi/4, 0, 1, 0, 0 ) )
	UNITTEST_FLOAT_EQUALS( sqrt(.5f), calcYawPitchY( pi/4, 0, 0, 1, 0 ) )
	UNITTEST_FLOAT_EQUALS( 0, calcYawPitchY( pi/4, 0, 0, 0, 1 ) )
	UNITTEST_FLOAT_EQUALS( 0, calcYawPitchY( 0, pi/4, 1, 0, 0 ) )
	UNITTEST_FLOAT_EQUALS( 1, calcYawPitchY( 0, pi/4, 0, 1, 0 ) )
	UNITTEST_FLOAT_EQUALS( 0, calcYawPitchY( 0, pi/4, 0, 0, 1 ) )
	UNITTEST_FLOAT_EQUALS( .5f, calcYawPitchY( pi/4, pi/4, 1, 0, 0 ) )
	UNITTEST_FLOAT_EQUALS( sqrt(.5f), calcYawPitchY( pi/4, pi/4, 0, 1, 0 ) )
	UNITTEST_FLOAT_EQUALS( .5f, calcYawPitchY( pi/4, pi/4, 0, 0, 1 ) )

	UNITTEST_FLOAT_EQUALS( 0, calcYawPitchZ( 0, 0, 1, 0, 0 ) )
	UNITTEST_FLOAT_EQUALS( 0, calcYawPitchZ( 0, 0, 0, 1, 0 ) )
	UNITTEST_FLOAT_EQUALS( 1, calcYawPitchZ( 0, 0, 0, 0, 1 ) )
	UNITTEST_FLOAT_EQUALS( 0, calcYawPitchZ( pi/4, 0, 1, 0, 0 ) )
	UNITTEST_FLOAT_EQUALS( 0, calcYawPitchZ( pi/4, 0, 0, 1, 0 ) )
	UNITTEST_FLOAT_EQUALS( 1, calcYawPitchZ( pi/4, 0, 0, 0, 1 ) )
	UNITTEST_FLOAT_EQUALS( -sqrt(.5f), calcYawPitchZ( 0, pi/4, 1, 0, 0 ) )
	UNITTEST_FLOAT_EQUALS( 0, calcYawPitchZ( 0, pi/4, 0, 1, 0 ) )
	UNITTEST_FLOAT_EQUALS( sqrt(.5f), calcYawPitchZ( 0, pi/4, 0, 0, 1 ) )
	UNITTEST_FLOAT_EQUALS( -sqrt(.5f), calcYawPitchZ( pi/4, pi/4, 1, 0, 0 ) )
	UNITTEST_FLOAT_EQUALS( 0, calcYawPitchZ( pi/4, pi/4, 0, 1, 0 ) )
	UNITTEST_FLOAT_EQUALS( sqrt(.5f), calcYawPitchZ( pi/4, pi/4, 0, 0, 1 ) )
}

/**
 * Set/Get a num,ber of different instance variables
 */
TESTSET( testsetGetSet )
{
	SweepDialog dlg;
	resetInstanceVariables(dlg);
	dlg.setValues( SweepDialog::ID_WINDX, 0.5f, 0.f, 1.f );
	dlg.setValues( SweepDialog::ID_WINDY, 0.6f, 0.f, 1.f );
	dlg.setValues( SweepDialog::ID_WINDZ, 0.7f, 0.f, 1.f );
	dlg.setValues( SweepDialog::ID_WINDYAW,   0.0f, -pi, pi );
	dlg.setValues( SweepDialog::ID_WINDPITCH, 0.0f, -pi, pi );
	dlg.setValues( SweepDialog::ID_CHANNEL1, 0.1f, -1.f, 1.f );
	dlg.setValues( SweepDialog::ID_CHANNEL2, 0.2f, -1.f, 1.f );
	dlg.setValues( SweepDialog::ID_CHANNEL3, 0.3f, -1.f, 1.f );

	UNITTEST_FLOAT_EQUALS( .5f, dlg.getValue( SweepDialog::ID_WINDX_VALUE ));
	UNITTEST_FLOAT_EQUALS( .6f, dlg.getValue( SweepDialog::ID_WINDY_VALUE ));
	UNITTEST_FLOAT_EQUALS( .7f, dlg.getValue( SweepDialog::ID_WINDZ_VALUE ));
	UNITTEST_FLOAT_EQUALS( .0f, dlg.getValue( SweepDialog::ID_WINDYAW_VALUE ));
	UNITTEST_FLOAT_EQUALS( .0f, dlg.getValue( SweepDialog::ID_WINDPITCH_VALUE ));
	UNITTEST_FLOAT_EQUALS( .1f, dlg.getValue( SweepDialog::ID_CHANNEL1_VALUE ));
	UNITTEST_FLOAT_EQUALS( .2f, dlg.getValue( SweepDialog::ID_CHANNEL2_VALUE ));
	UNITTEST_FLOAT_EQUALS( .3f, dlg.getValue( SweepDialog::ID_CHANNEL3_VALUE ));

	UNITTEST_FLOAT_EQUALS( 0.5f, dlg.getInterpolatedValue( SweepDialog::ID_WINDX, 30, 61 ));
	UNITTEST_FLOAT_EQUALS( .3f, dlg.getInterpolatedValue( SweepDialog::ID_WINDY, 3, 11 ));
	UNITTEST_FLOAT_EQUALS( .7f, dlg.getInterpolatedValue( SweepDialog::ID_WINDZ, 7, 11 ));
	UNITTEST_FLOAT_EQUALS( pi/2, dlg.getInterpolatedValue( SweepDialog::ID_WINDYAW, 15, 21 ));
	UNITTEST_FLOAT_EQUALS( -pi/2, dlg.getInterpolatedValue( SweepDialog::ID_WINDPITCH, 5, 21 ));
	UNITTEST_FLOAT_EQUALS( .0, dlg.getInterpolatedValue( SweepDialog::ID_CHANNEL1, 50, 101 ));
	UNITTEST_FLOAT_EQUALS( .5, dlg.getInterpolatedValue( SweepDialog::ID_CHANNEL2, 75, 101 ));
	UNITTEST_FLOAT_EQUALS( -.5, dlg.getInterpolatedValue( SweepDialog::ID_CHANNEL3, 25, 101 ));
	UNITTEST_FLOAT_EQUALS( -.25, dlg.getInterpolatedValue( SweepDialog::ID_CHANNEL2, 75, 201 ));
	UNITTEST_FLOAT_EQUALS( -.75, dlg.getInterpolatedValue( SweepDialog::ID_CHANNEL3, 25, 201 ));


	ModelSimulator_environment_t environment;
	ModelSimulator_inertias_t inertias;

	dlg.get( environment, inertias, 75 );
	UNITTEST_FLOAT_EQUALS( .5f, environment.linearVelocityX );
	UNITTEST_FLOAT_EQUALS( .6f, environment.linearVelocityY );
	UNITTEST_FLOAT_EQUALS( .7f, environment.linearVelocityZ );
	UNITTEST_FLOAT_EQUALS( .5f, environment.channel[0] );
	UNITTEST_FLOAT_EQUALS( .2f, environment.channel[1] );
	UNITTEST_FLOAT_EQUALS( .3f, environment.channel[2] );
}

/**
 * Sweep (1,0,0) around the yaw
 */
TESTSET( testsetYaw )
{
	//
	// Create a dialog instance, filled in with known values
	//
	SweepDialog dlg;
	resetInstanceVariables(dlg);
	dlg.setValues( SweepDialog::ID_WINDX, 1.f, 0.f, 1.f );
	dlg.setValues( SweepDialog::ID_WINDY, 0.f, 0.f, 1.f );
	dlg.setValues( SweepDialog::ID_WINDZ, 0.f, 0.f, 1.f );
	dlg.setValues( SweepDialog::ID_WINDYAW,   0.0f, -pi, pi );
	dlg.setValues( SweepDialog::ID_WINDPITCH, 0.0f, -pi, pi );
	dlg.setSelectedInput( SweepDialog::ID_WINDYAW );
	dlg.setNumberOfSteps( 81 );

	//
	// Run a bunch of known cases
	//
	ModelSimulator_environment_t environment; // contains the result to check
	ModelSimulator_inertias_t inertias; // never used, but needed when calling dlg.get.

	dlg.get( environment, inertias, 0 );
	UNITTEST_FLOAT_EQUALS( -1.f, environment.linearVelocityX );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityY );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityZ );

	dlg.get( environment, inertias, 10 );
	UNITTEST_FLOAT_EQUALS( -sqrt(.5f), environment.linearVelocityX );
	UNITTEST_FLOAT_EQUALS( -sqrt(.5f), environment.linearVelocityY );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityZ );

	dlg.get( environment, inertias, 20 );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityX );
	UNITTEST_FLOAT_EQUALS( -1.f, environment.linearVelocityY );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityZ );

	dlg.get( environment, inertias, 30 );
	UNITTEST_FLOAT_EQUALS( sqrt(.5f), environment.linearVelocityX );
	UNITTEST_FLOAT_EQUALS( -sqrt(.5f), environment.linearVelocityY );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityZ );

	dlg.get( environment, inertias, 40 );
	UNITTEST_FLOAT_EQUALS( 1.f, environment.linearVelocityX );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityY );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityZ );

	dlg.get( environment, inertias, 50 );
	UNITTEST_FLOAT_EQUALS( sqrt(.5f), environment.linearVelocityX );
	UNITTEST_FLOAT_EQUALS( sqrt(.5f), environment.linearVelocityY );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityZ );

	dlg.get( environment, inertias, 60 );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityX );
	UNITTEST_FLOAT_EQUALS( 1.f, environment.linearVelocityY );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityZ );

	dlg.get( environment, inertias, 70 );
	UNITTEST_FLOAT_EQUALS( -sqrt(.5f), environment.linearVelocityX );
	UNITTEST_FLOAT_EQUALS( sqrt(.5f), environment.linearVelocityY );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityZ );

	dlg.get( environment, inertias, 80 );
	UNITTEST_FLOAT_EQUALS( -1.f, environment.linearVelocityX );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityY );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityZ );
}

/**
 * Sweep (1,0,0) around the pitch
 */
TESTSET( testsetPitch )
{	
	//
	// Create a dialog instance, filled in with known values
	//
	SweepDialog dlg;
	resetInstanceVariables(dlg);
	dlg.setValues( SweepDialog::ID_WINDX, 1.f, 0.f, 1.f );
	dlg.setValues( SweepDialog::ID_WINDY, 0.f, 0.f, 1.f );
	dlg.setValues( SweepDialog::ID_WINDZ, 0.f, 0.f, 1.f );
	dlg.setValues( SweepDialog::ID_WINDYAW,   0.0f, -pi, pi );
	dlg.setValues( SweepDialog::ID_WINDPITCH, 0.0f, -pi, pi );
	dlg.setSelectedInput( SweepDialog::ID_WINDPITCH );
	dlg.setNumberOfSteps( 81 );

	//
	// Run a bunch of known cases
	//
	ModelSimulator_environment_t environment; // contains the result to check
	ModelSimulator_inertias_t inertias; // never used, but needed when calling dlg.get.

	dlg.get( environment, inertias, 0 );
	UNITTEST_FLOAT_EQUALS( -1.f, environment.linearVelocityX );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityY );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityZ );

	dlg.get( environment, inertias, 10 );
	UNITTEST_FLOAT_EQUALS( -sqrt(.5f), environment.linearVelocityX );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityY );
	UNITTEST_FLOAT_EQUALS( sqrt(.5f), environment.linearVelocityZ );

	dlg.get( environment, inertias, 20 );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityX );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityY );
	UNITTEST_FLOAT_EQUALS( 1.f, environment.linearVelocityZ );

	dlg.get( environment, inertias, 30 );
	UNITTEST_FLOAT_EQUALS( sqrt(.5f), environment.linearVelocityX );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityY );
	UNITTEST_FLOAT_EQUALS( sqrt(.5f), environment.linearVelocityZ );

	dlg.get( environment, inertias, 40 );
	UNITTEST_FLOAT_EQUALS( 1.f, environment.linearVelocityX );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityY );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityZ );

	dlg.get( environment, inertias, 50 );
	UNITTEST_FLOAT_EQUALS( sqrt(.5f), environment.linearVelocityX );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityY );
	UNITTEST_FLOAT_EQUALS( -sqrt(.5f), environment.linearVelocityZ );

	dlg.get( environment, inertias, 60 );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityX );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityY );
	UNITTEST_FLOAT_EQUALS( -1.f, environment.linearVelocityZ );

	dlg.get( environment, inertias, 70 );
	UNITTEST_FLOAT_EQUALS( -sqrt(.5f), environment.linearVelocityX );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityY );
	UNITTEST_FLOAT_EQUALS( -sqrt(.5f), environment.linearVelocityZ );

	dlg.get( environment, inertias, 80 );
	UNITTEST_FLOAT_EQUALS( -1.f, environment.linearVelocityX );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityY );
	UNITTEST_FLOAT_EQUALS( 0.f, environment.linearVelocityZ );
}

/** 
 * Unit test of the select(ModelSimulator_forces_t & ) function
 */
TESTSET( testsetSelectForces )
{
	//ModelSimulator_environment_t environment = { sizeof(ModelSimulator_environment_t) };
	//ModelSimulator_inertias_t inertias = { sizeof(ModelSimulator_inertias_t) };

	//
	// Create a forces structure, with known values
	//
	ModelSimulator_forces_t forces;
	forces.size = sizeof forces;
	forces.force_x = 11.f;
	forces.force_y = 22.f;
	forces.force_z = 33.f;
	forces.torque_x = 44.f;
	forces.torque_y = 55.f;
	forces.torque_z = 66.f;

	//
	// Create a dialog instance, don't really care about which values it has
	//
	SweepDialog dlg; // use the test version of the contructor
	resetInstanceVariables(dlg);

	//
	// Run a bunch of known cases
	//

	dlg.setSelectedOutput( SweepDialog::ID_FORCE_POSX );
	UNITTEST_FLOAT_EQUALS( 11.f, dlg.select(forces) );

	dlg.setSelectedOutput( SweepDialog::ID_FORCE_POSY );
	UNITTEST_FLOAT_EQUALS( 22.f, dlg.select(forces) );

	dlg.setSelectedOutput( SweepDialog::ID_FORCE_POSZ );
	UNITTEST_FLOAT_EQUALS( 33.f, dlg.select(forces) );

	dlg.setSelectedOutput( SweepDialog::ID_FORCE_NEGX );
	UNITTEST_FLOAT_EQUALS( -11.f, dlg.select(forces) );

	dlg.setSelectedOutput( SweepDialog::ID_FORCE_NEGY );
	UNITTEST_FLOAT_EQUALS( -22.f, dlg.select(forces) );

	dlg.setSelectedOutput( SweepDialog::ID_FORCE_NEGZ );
	UNITTEST_FLOAT_EQUALS( -33.f, dlg.select(forces) );

	dlg.setSelectedOutput( SweepDialog::ID_TORQUE_POSX );
	UNITTEST_FLOAT_EQUALS( 44.f, dlg.select(forces) );

	dlg.setSelectedOutput( SweepDialog::ID_TORQUE_POSY );
	UNITTEST_FLOAT_EQUALS( 55.f, dlg.select(forces) );

	dlg.setSelectedOutput( SweepDialog::ID_TORQUE_POSZ );
	UNITTEST_FLOAT_EQUALS( 66.f, dlg.select(forces) );

	dlg.setSelectedOutput( SweepDialog::ID_TORQUE_NEGX );
	UNITTEST_FLOAT_EQUALS( -44.f, dlg.select(forces) );

	dlg.setSelectedOutput( SweepDialog::ID_TORQUE_NEGY );
	UNITTEST_FLOAT_EQUALS( -55.f, dlg.select(forces) );

	dlg.setSelectedOutput( SweepDialog::ID_TORQUE_NEGZ );
	UNITTEST_FLOAT_EQUALS( -66.f, dlg.select(forces) );

}

/** 
 * Unit test of the select(int) function
 */
TESTSET( testsetSelectInt )
{
	//
	// Create a dialog instance, filled in with known values
	//
	SweepDialog dlg; // use the test version of the contructor
	resetInstanceVariables(dlg);
	dlg.setValues( SweepDialog::ID_WINDX, 1.f, 0.f, 1.f );
	dlg.setValues( SweepDialog::ID_WINDY, 0.f, 1.f, 0.f );
	dlg.setValues( SweepDialog::ID_WINDZ, 0.f, 0.f, 1.f );
	dlg.setValues( SweepDialog::ID_WINDYAW,   0.0f, -pi, pi );
	dlg.setValues( SweepDialog::ID_WINDPITCH, 0.0f, -pi, pi );
	dlg.setValues( SweepDialog::ID_CHANNEL1, 0.1f, -1.f, 1.f );
	dlg.setValues( SweepDialog::ID_CHANNEL2, 0.2f, -1.f, 1.f );
	dlg.setValues( SweepDialog::ID_CHANNEL3, 0.3f, -1.f, 1.f );
	dlg.setNumberOfSteps( 81 );

	//
	// Select channel 1 as input, and check some known cases
	//
	dlg.setSelectedInput( SweepDialog::ID_CHANNEL1 );
	UNITTEST_FLOAT_EQUALS( -1.f, dlg.select(0) );
	UNITTEST_FLOAT_EQUALS( -.5f, dlg.select(20) );
	UNITTEST_FLOAT_EQUALS( 0.f, dlg.select(40) );
	UNITTEST_FLOAT_EQUALS( .5f, dlg.select(60) );
	UNITTEST_FLOAT_EQUALS( 1.f, dlg.select(80) );
	
	//
	// Select channel 2 as input, and check some known cases
	//
	dlg.setSelectedInput( SweepDialog::ID_CHANNEL2 );
	UNITTEST_FLOAT_EQUALS( -1.f, dlg.select(0) );
	UNITTEST_FLOAT_EQUALS( -.75f, dlg.select(10) );
	UNITTEST_FLOAT_EQUALS( 0.f, dlg.select(40) );
	UNITTEST_FLOAT_EQUALS( .75f, dlg.select(70) );
	UNITTEST_FLOAT_EQUALS( 1.f, dlg.select(80) );

	//
	// Select wind_x as input, and check some known cases
	//
	dlg.setSelectedInput( SweepDialog::ID_WINDX );
	UNITTEST_FLOAT_EQUALS( 0.f, dlg.select(0) );
	UNITTEST_FLOAT_EQUALS( .25f, dlg.select(20) );
	UNITTEST_FLOAT_EQUALS( .5f, dlg.select(40) );
	UNITTEST_FLOAT_EQUALS( .75f, dlg.select(60) );
	UNITTEST_FLOAT_EQUALS( 1.f, dlg.select(80) );

	//
	// Select wind_y as input, and check some known cases
	//
	dlg.setSelectedInput( SweepDialog::ID_WINDY );
	UNITTEST_FLOAT_EQUALS( 1.f, dlg.select(0) );
	UNITTEST_FLOAT_EQUALS( .75f, dlg.select(20) );
	UNITTEST_FLOAT_EQUALS( .5f, dlg.select(40) );
	UNITTEST_FLOAT_EQUALS( .25f, dlg.select(60) );
	UNITTEST_FLOAT_EQUALS( 0.f, dlg.select(80) );

	//
	// Select wind_yaw as input, and check some known cases
	//
	dlg.setSelectedInput( SweepDialog::ID_WINDYAW );
	UNITTEST_FLOAT_EQUALS( -pi, dlg.select(0) );
	UNITTEST_FLOAT_EQUALS( -pi/2, dlg.select(20) );
	UNITTEST_FLOAT_EQUALS( 0, dlg.select(40) );
	UNITTEST_FLOAT_EQUALS( pi/2, dlg.select(60) );
	UNITTEST_FLOAT_EQUALS( pi, dlg.select(80) );

	//
	// Select wind_pitch as input, and check some known cases
	//
	dlg.setSelectedInput( SweepDialog::ID_WINDPITCH );
	UNITTEST_FLOAT_EQUALS( -pi, dlg.select(0) );
	UNITTEST_FLOAT_EQUALS( -3*pi/4, dlg.select(10) );
	UNITTEST_FLOAT_EQUALS( 0, dlg.select(40) );
	UNITTEST_FLOAT_EQUALS( pi*3/4, dlg.select(70) );
	UNITTEST_FLOAT_EQUALS( pi, dlg.select(80) );
}

// TODO: make RUN_TESTSET(...) below
// not to appear in the doxygen documentation
BEGIN_TESTING()
	RUN_TESTSET( testsetInterpolate );
	RUN_TESTSET( testsetParseFloatString );
	RUN_TESTSET( testsetCalcYawPitch );
	RUN_TESTSET( testsetGetSet );
	RUN_TESTSET( testsetYaw );
	RUN_TESTSET( testsetPitch );
	RUN_TESTSET( testsetSelectForces );
	RUN_TESTSET( testsetSelectInt );
END_TESTING()







